//#ifdef __cplusplus
//extern "C" {
//#endif

#include <stdio.h>
#include <stdlib.h>
#include <time.h>

#define PRINT_DBL_FMT "%+.6f "

#ifdef USE_DSPLIB
  #include "src/DSP_mat_mul/DSP_mat_mul.h"
#endif

#define MTX double **
#define VECT double *

extern MTX alloc_MTX(int rows, int cols);
extern void dealloc_MTX(int rows, MTX matrix);

extern VECT alloc_VECT(int len);

extern void mm_mult(int rows_a, int cols_a, MTX A,
                    int cols_b, MTX B, MTX C);

// Calculates the product of matrix and vector
// Result will be placed to R argument
// WARNING! There's should be enough space in R vector
extern void mv_mult(int rows_a, int cols_a, MTX A,
                    VECT V, VECT R);

// Calculates the summ of two vectors
// Result will be placed to Va argument
extern void vv_sum_accum(int len, VECT Va, VECT Vb);

extern void lu_solve_4_1(MTX a, VECT z, VECT res);

//#ifndef MTX
//    #define MTX double **
//#endif
//#ifndef VECT
//    #define VECT double *
//#endif


MTX alloc_MTX(int rows, int cols)
{
    int i;

    MTX res = (MTX)calloc(rows, sizeof(VECT));

    for (i = 0; i < rows; ++i) {
        res[i] = (VECT)calloc(cols, sizeof(double));
    }

    return res;
}
void dealloc_MTX(int rows, MTX matrix)
{
    int i;
    for (i = 0; i < rows; ++i) {
        free(matrix[i]);
    }
    free(matrix);
}

VECT alloc_VECT(int len)
{
    return (VECT)calloc(len, sizeof(double));
}

// Random matrix fill
void matfill(int rows, int cols, MTX mat)
{
  int i, j;
  for (i = 0; i < rows; i++)
  {
    for (j = 0; j < cols; j++)
    {
      mat[i][j] = (double) ((rand() % 1000000) / 1000.0);
    }
  }
}

// Random vector fill
void vectfill(int len, VECT vect)
{
  int i;
  for (i = 0; i < len; i++)
  {
    vect[i] = (double) ((rand() % 1000000) / 1000.0);
  }
}

void printmat(int rows, int cols, MTX mat)
{
  int i, j;
  for (i = 0; i < rows; i++)
  {
    for (j = 0; j < cols; j++)
    {
      printf(PRINT_DBL_FMT, mat[i][j]);
    }
    printf("\n");
  }
}

void printvect(int len, VECT vect)
{
  int i;
  for (i = 0; i < len; i++)
  {
    printf(PRINT_DBL_FMT, vect[i]);
  }
  printf("\n");
}

// Calculates the summ of two vectors
// Result will be placed to Va argument
void vv_sum_accum(int len, VECT Va, VECT Vb)
{
    int i;
    for (i = 0; i < len; ++i) {
        Va[i] = Va[i] + Vb[i];
    }
}

// Matrix multiply using Fixed point C64x+ DSPLib function
void mm_mult(int rows_a, int cols_a, MTX A,
                    int cols_b, MTX B, MTX C)
{
#ifdef USE_DSPLIB
  DSP_mat_mul( A,rows_a,cols_a,
               B,cols_b,
               C,
               0);
#else
  int i, j, k;
  double sum;

  for (i = 0; i < rows_a; i++)
  {
    for (j = 0; j < cols_b; j++)
    {
      sum = 0;
      for (k = 0; k < cols_a; k++)
      {
        sum += (A[i][k] * B[k][j]);
      }
      C[i][j] = sum;
    }
  }
#endif
}

void mm_mult_a_transp(int rows_a, int cols_a, MTX A,
                    int cols_b, MTX B, MTX C)
{
//#ifdef USE_DSPLIB
//  DSP_mat_mul( A,rows_a,cols_a,
//               B,cols_b,
//               C,
//               0);
//#else
  int i, j, k;
  double sum;

  for (i = 0; i < cols_a; i++)
  {
    for (j = 0; j < cols_b; j++)
    {
      sum = 0;
      for (k = 0; k < rows_a; k++)
      {
        sum += (A[k][i] * B[k][j]);
      }
      C[i][j] = sum;
    }
  }
//#endif
}

void mm_mult_b_transp(int rows_a, int cols_a, MTX A,
                    int rows_b, MTX B, MTX C)
{
//#ifdef USE_DSPLIB
//  DSP_mat_mul( A,rows_a,cols_a,
//               B,cols_b,
//               C,
//               0);
//#else
  int i, j, k;
  double sum;

  for (i = 0; i < rows_a; i++)
  {
    for (j = 0; j < rows_b; j++)
    {
      sum = 0;
      for (k = 0; k < cols_a; k++)
      {
        sum += (A[i][k] * B[j][k]);
      }
      C[i][j] = sum;
    }
  }
//#endif
}
void mv_mult(int rows_a, int cols_a, MTX A,
             VECT V, VECT R)
{
    int i, j;
    double sum;

    for (i = 0; i < rows_a; i++)
    {
        sum = 0;
        for (j = 0; j < cols_a; j++)
        {
            sum += (A[i][j] * V[j]);
        }
        R[i] = sum;
    }
}

void mm_add_sub(int rows, int cols, MTX a, MTX b, MTX res, int positive)
{
    int i,j;
    for (i = 0; i < rows; ++i) {
        for (j = 0; j < cols; ++j) {
            res[i][j] = a[i][j] + positive ? b[i][j] : -b[i][j];
        }
    }
}

void lu_solve_4_1(MTX a, VECT z, VECT res)
{
    double t1 = a[0][0]*a[1][1]-a[0][1]*a[1][0];
    double t2 = a[0][0]*a[1][3]-a[0][3]*a[1][0];
    double t3 = a[2][2]*a[3][3]-a[2][3]*a[3][2];
    double t4 = a[2][1]*a[3][2]-a[2][2]*a[3][1];
    double t5 = a[0][2]*a[1][0]-a[0][0]*a[1][2];
    double t6 = a[0][2]*a[1][3]-a[0][3]*a[1][2];
    double t7 = a[0][2]*a[1][1]-a[0][1]*a[1][2];
    double t8 = a[0][1]*a[1][3]-a[0][3]*a[1][1];
    double t9 = a[2][0]*a[3][3]-a[2][3]*a[3][0];
    double t10 = a[2][0]*a[3][2]-a[2][2]*a[3][0];
    double t11 = a[2][1]*a[3][0]-a[2][0]*a[3][1];
    double t12 = a[2][3]*a[3][1]-a[2][1]*a[3][3];
    double t13 = a[2][0]*z[3]-a[3][0]*z[2];
    double t14 = a[0][3]*a[2][0]-a[0][0]*a[2][3];
    double t15 = a[1][0]*a[2][3]-a[1][3]*a[2][0];
    double t16 = a[2][2]*z[3]-a[3][2]*z[2];
    double t17 = a[2][3]*z[3]-a[3][3]*z[2];
    double t18 = a[3][1]*z[2]-a[2][1]*z[3];
    double t19 = a[0][2]*a[2][1]-a[0][1]*a[2][2];
    double t20 = a[0][1]*a[2][0]-a[0][0]*a[2][1];
    double t21 = a[0][1]*a[2][3]-a[0][3]*a[2][1];
    double t22 = a[0][3]*a[2][2]-a[0][2]*a[2][3];
    double t23 = a[0][0]*a[2][2]-a[0][2]*a[2][0];
    double t24 = a[1][2]*a[2][3]-a[1][3]*a[2][2];
    double t25 = a[1][1]*a[2][2]-a[1][2]*a[2][1];
    double t26 = a[1][3]*a[2][1]-a[1][1]*a[2][3];
    double t27 = a[1][0]*a[2][1]-a[1][1]*a[2][0];
    double t28 = a[1][2]*a[2][0]-a[1][0]*a[2][2];
    double t29 = a[3][2]*z[0];
    double t30 = a[3][3]*z[0];
    double t31 = a[3][1]*z[0];
    double t32 = a[3][0]*z[0];
    double t33 = a[3][1]*z[1];
    double t34 = a[3][2]*z[1];
    double t35 = a[3][3]*z[1];
    double t36 = a[3][0]*z[1];

    double d = t7*t9+t10*t8+t11*t6+t12*t5-t2*t4-t1*t3;

    res[0] = -(t16*t8+t17*t7+t18*t6+t19*t35+t21*t34+t22*t33+t24*t31+t25*t30+t26*t29)/d;
    res[1] = -(t13*t6-t17*t5-t22*t36+t23*t35+t14*t34-t24*t32+t28*t30+t15*t29-t16*t2)/d;
    res[2] = (t13*t8+t21*t36-t20*t35+t14*t33+t26*t32+t15*t31-t27*t30+t18*t2+t1*t17)/d;
    res[3] = (t13*t7+t18*t5+t19*t36+t20*t34+t23*t33+t25*t32+t28*t31+t27*t29-t1*t16)/d;
}

MTX lu_solve_4_n(MTX a, int cols_b, MTX b, MTX res)
{
    int i;

    //MTX res= alloc_MTX(4, cols_b);//[4][cols_b]

    for (i = 0; i < cols_b; ++i) {
        double col[] = { b[0][i], b[1][i], b[2][i], b[3][i] };
        lu_solve_4_1(a, col, col);

        res[0][i] = col[0];
        res[1][i] = col[1];
        res[2][i] = col[2];
        res[3][i] = col[3];
    }

    return res;
}

void lu_solve_3_n_b_transp(MTX a, int rows_b, MTX b, MTX res)
{
    double t1 = a[0][0]*a[1][1]-a[0][1]*a[1][0];
    double t2 = a[0][2]*a[1][0]-a[0][0]*a[1][2];
    double t3 = a[0][1]*a[1][2]-a[0][2]*a[1][1];
    double t4 = a[0][2]*a[2][1]-a[0][1]*a[2][2];
    double t5 = a[1][1]*a[2][2]-a[1][2]*a[2][1];
    double t6 = a[0][0]*a[2][2]-a[0][2]*a[2][0];
    double t7 = a[1][2]*a[2][0]-a[1][0]*a[2][2];
    double t8 = a[0][1]*a[2][0]-a[0][0]*a[2][1];
    double t9 = a[1][0]*a[2][1]-a[1][1]*a[2][0];

    double d = a[2][0]*t3+a[2][1]*t2+a[2][2]*t1;

    int i;
    for (i = 0; i < rows_b; ++i) { // We must use transposed MTX b => Walking by rows instead of columns
        // z0*T5+z1*T4+z2*T3 : D
        res[0][i] = (b[i][0]*t5 + b[1][i]*t4 + b[i][2]*t3)/d;

        // z0*T7+z1*T6+z2*T2 : D
        res[1][i] = (b[i][0]*t7 + b[i][1]*t6 + b[i][2]*t2)/d;

        // z0*T9+z1*T8+z2*T1 : D
        res[2][i] = (b[i][0]*t9 + b[i][1]*t8 + b[i][2]*t1)/d;
    }
}


//#ifdef __cplusplus
//}
//#endif
